/**
 * @file dc_motor_control.c
 * @brief 直流电机高级控制函数实现
 * @author Claude
 * @date 2024-05-25
 *
 * @details
 * 基于EAM20XX电机控制函数库的GM37-520直流电机控制实现
 * 
 * 速度单位说明:
 * 1. 编码器测量的速度单位为RPM(每分钟转数)
 * 2. 控制系统内部使用标准化单位(0.0-1.0)表示占空比
 * 3. 使用动态估计确保前后轮速度一致
 */

#include "dc_motor_control.h"
#include <stdint.h>

#define CONTROL_PERIOD_MS 10

// 电机校准系数（可根据实际情况调整）
#define MOTOR1_CAL_FACTOR 1.0f
#define MOTOR2_CAL_FACTOR 1.0f
#define MOTOR3_CAL_FACTOR 1.0f
#define MOTOR4_CAL_FACTOR 1.0f

// 定义固定占空比
#define FIXED_DUTY_CYCLE 0.2f

static uint32_t g_u32LastControlTime = 0;

// 速度参考值
static float g_motorSpeedRef[4] = {0, 0, 0, 0};

void DCMotor_Init(void)
{
    InitAllPWM();
    g_u32LastControlTime = 0;
}

void DCMotor_SetSpeedRef(uint8_t motorIndex, float speedRef)
{
    if (motorIndex < 4) {
        if (speedRef > 1.0f) speedRef = 1.0f;
        if (speedRef < -1.0f) speedRef = -1.0f;
        g_motorSpeedRef[motorIndex] = speedRef;
    }
}

static void DCMotor_SetMotorSpeed(uint8_t motorIndex, float speed)
{
    uint8_t direction = (speed >= 0.0f) ? FORWARD : BACKWARD;
    uint16_t duty = (uint16_t)( (speed < 0.0f ? -speed : speed) * 0xFFFF );
    switch(motorIndex) {
        case 0: SetMotor1Direction(direction); SetMotor1Speed(duty); break;
        case 1: SetMotor2Direction(direction); SetMotor2Speed(duty); break;
        case 2: SetMotor3Direction(direction); SetMotor3Speed(duty); break;
        case 3: SetMotor4Direction(direction); SetMotor4Speed(duty); break;
    }
}

void DCMotor_UpdateAllMotors(void)
{
    for (uint8_t i = 0; i < 4; i++) {
        DCMotor_SetMotorSpeed(i, g_motorSpeedRef[i]);
    }
}

void DCMotor_Stop(void)
{
    for (int i = 0; i < 4; i++) {
        DCMotor_SetSpeedRef(i, 0.0f);
    }
    StopAllMotors();
}

/**
 * @brief 直行
 * @param speed 速度(0.0-1.0)
 */
void DCMotor_MoveForward(float speed)
{
    // 限制速度范围
    if (speed > 1.0f) {
        speed = 1.0f;
    } else if (speed < 0.0f) {
        speed = 0.0f;
    }
    
    // 确保所有电机方向正确设置为前进
    SetMotor1Direction(FORWARD);
    SetMotor2Direction(FORWARD);
    SetMotor3Direction(FORWARD);
    SetMotor4Direction(FORWARD);
    
    // 应用校准系数，使所有电机实际速度一致
    DCMotor_SetSpeedRef(0, speed * MOTOR1_CAL_FACTOR);
    DCMotor_SetSpeedRef(1, speed * MOTOR2_CAL_FACTOR);
    DCMotor_SetSpeedRef(2, speed * MOTOR3_CAL_FACTOR);
    DCMotor_SetSpeedRef(3, speed * MOTOR4_CAL_FACTOR);
}

/**
 * @brief 后退
 * @param speed 速度(0.0-1.0)
 */
void DCMotor_MoveBackward(float speed)
{
    // 限制速度范围
    if (speed > 1.0f) {
        speed = 1.0f;
    } else if (speed < 0.0f) {
        speed = 0.0f;
    }
    
    // 确保所有电机方向正确设置为后退
    SetMotor1Direction(BACKWARD);
    SetMotor2Direction(BACKWARD);
    SetMotor3Direction(BACKWARD);
    SetMotor4Direction(BACKWARD);
    
    // 估算后轮在固定占空比下的速度（用于前轮闭环控制参考）
    // g_f32RearWheelRefSpeed = 15.0f * speed; // 简单线性估计
    
    // 应用校准系数，使所有电机实际速度一致
    DCMotor_SetSpeedRef(0, -speed * MOTOR1_CAL_FACTOR);
    DCMotor_SetSpeedRef(1, -speed * MOTOR2_CAL_FACTOR);
    DCMotor_SetSpeedRef(2, -speed * MOTOR3_CAL_FACTOR);
    DCMotor_SetSpeedRef(3, -speed * MOTOR4_CAL_FACTOR);
}

/**
 * @brief 左转
 * @param speed 速度(0.0-1.0)
 */
void DCMotor_TurnLeft(float speed)
{
    // 限制速度范围
    if (speed > 1.0f) {
        speed = 1.0f;
    } else if (speed < 0.0f) {
        speed = 0.0f;
    }
    
    // 明确设置各电机方向
    SetMotor1Direction(FORWARD);  // 右后轮前进
    SetMotor2Direction(FORWARD);  // 右前轮前进
    SetMotor3Direction(BACKWARD); // 左前轮后退
    SetMotor4Direction(BACKWARD); // 左后轮后退
    
    // 左侧电机反转，应用校准系数
    DCMotor_SetSpeedRef(2, -speed * MOTOR3_CAL_FACTOR * 0.7f); // 左前轮
    DCMotor_SetSpeedRef(3, -speed * MOTOR4_CAL_FACTOR * 0.7f); // 左后轮
    
    // 右侧电机正转，应用校准系数
    DCMotor_SetSpeedRef(0, speed * MOTOR1_CAL_FACTOR);        // 右后轮
    DCMotor_SetSpeedRef(1, speed * MOTOR2_CAL_FACTOR);        // 右前轮
}

/**
 * @brief 右转
 * @param speed 速度(0.0-1.0)
 */
void DCMotor_TurnRight(float speed)
{
    // 限制速度范围
    if (speed > 1.0f) {
        speed = 1.0f;
    } else if (speed < 0.0f) {
        speed = 0.0f;
    }
    
    // 明确设置各电机方向
    SetMotor1Direction(BACKWARD); // 右后轮后退
    SetMotor2Direction(BACKWARD); // 右前轮后退
    SetMotor3Direction(FORWARD);  // 左前轮前进
    SetMotor4Direction(FORWARD);  // 左后轮前进
    
    // 右侧电机反转，应用校准系数
    DCMotor_SetSpeedRef(0, -speed * MOTOR1_CAL_FACTOR * 0.7f); // 右后轮
    DCMotor_SetSpeedRef(1, -speed * MOTOR2_CAL_FACTOR * 0.7f); // 右前轮
    
    // 左侧电机正转，应用校准系数
    DCMotor_SetSpeedRef(2, speed * MOTOR3_CAL_FACTOR);        // 左前轮
    DCMotor_SetSpeedRef(3, speed * MOTOR4_CAL_FACTOR);        // 左后轮
}

/**
 * @brief 控制小车向左平移
 * @param speed 速度参考值
 */
void DCMotor_MoveLeft(float speed)
{
    // 设置电机方向和使能
    PINS_DRV_WritePin(MOTOR1_AIN1_PORT, MOTOR1_AIN1_PIN, 1); // 电机1反转
    PINS_DRV_WritePin(MOTOR1_AIN2_PORT, MOTOR1_AIN2_PIN, 0);
    
    PINS_DRV_WritePin(MOTOR2_AIN1_PORT, MOTOR2_AIN1_PIN, 0); // 电机2正转
    PINS_DRV_WritePin(MOTOR2_AIN2_PORT, MOTOR2_AIN2_PIN, 1);
    
    PINS_DRV_WritePin(MOTOR3_AIN1_PORT, MOTOR3_AIN1_PIN, 1); // 电机3反转
    PINS_DRV_WritePin(MOTOR3_AIN2_PORT, MOTOR3_AIN2_PIN, 0);
    
    PINS_DRV_WritePin(MOTOR4_AIN1_PORT, MOTOR4_AIN1_PIN, 0); // 电机4正转
    PINS_DRV_WritePin(MOTOR4_AIN2_PORT, MOTOR4_AIN2_PIN, 1);
    
    // 设置速度参考值
    g_motorSpeedRef[0] = speed;
    g_motorSpeedRef[1] = speed;
    g_motorSpeedRef[2] = speed;
    g_motorSpeedRef[3] = speed;
}

/**
 * @brief 控制小车向右平移
 * @param speed 速度参考值
 */
void DCMotor_MoveRight(float speed)
{
    // 设置电机方向和使能
    PINS_DRV_WritePin(MOTOR1_AIN1_PORT, MOTOR1_AIN1_PIN, 0); // 电机1正转
    PINS_DRV_WritePin(MOTOR1_AIN2_PORT, MOTOR1_AIN2_PIN, 1);
    
    PINS_DRV_WritePin(MOTOR2_AIN1_PORT, MOTOR2_AIN1_PIN, 1); // 电机2反转
    PINS_DRV_WritePin(MOTOR2_AIN2_PORT, MOTOR2_AIN2_PIN, 0);
    
    PINS_DRV_WritePin(MOTOR3_AIN1_PORT, MOTOR3_AIN1_PIN, 0); // 电机3正转
    PINS_DRV_WritePin(MOTOR3_AIN2_PORT, MOTOR3_AIN2_PIN, 1);
    
    PINS_DRV_WritePin(MOTOR4_AIN1_PORT, MOTOR4_AIN1_PIN, 1); // 电机4反转
    PINS_DRV_WritePin(MOTOR4_AIN2_PORT, MOTOR4_AIN2_PIN, 0);
    
    // 设置速度参考值
    g_motorSpeedRef[0] = speed;
    g_motorSpeedRef[1] = speed;
    g_motorSpeedRef[2] = speed;
    g_motorSpeedRef[3] = speed;
} 
